#ifndef G2O_PNP_EDGE_H_
#define G2O_PNP_EDGE_H_

#include "g2o_pnp_vertex.h"

// edge used in g2o ba
//   - edge: (_point_3d, _measurement)
//   - H = J'J, g=-J'e, dx = H.ldlt().solve(g) in GN
//   - J: _jacobianOplusXi
//   - e: _error
//   - dx as a update is injected to the vertex
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeProjection(const Eigen::Vector3d &point_3d,
                   const Eigen::Matrix3d &K)
        : _point_3d(point_3d), _K(K) {}

    // compute reprojection error
    virtual void computeError() override
    {
        const VertexPose *v = static_cast<VertexPose *>(_vertices[0]);
        Sophus::SE3d T = v->estimate();
        Eigen::Vector3d reproject_uv = _K * (T * _point_3d);
        reproject_uv /= reproject_uv[2];
        _error = _measurement - reproject_uv.head<2>();
    }

    // compute jacobian J
    virtual void linearizeOplus() override
    {
        const VertexPose *v = static_cast<VertexPose *>(_vertices[0]);
        Sophus::SE3d T = v->estimate();
        Eigen::Vector3d transform_point = T * _point_3d;
        double fx = _K(0, 0);
        double fy = _K(1, 1);
        double cx = _K(0, 2);
        double cy = _K(1, 2);
        double X = transform_point[0];
        double Y = transform_point[1];
        double Z = transform_point[2];
        double Z2 = Z * Z;
        _jacobianOplusXi
            << -fx / Z,
            0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
            0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
    }

    virtual bool read(istream &in) override { return false; }
    virtual bool write(ostream &out) const override { return false; }

private:
    Eigen::Vector3d _point_3d;
    Eigen::Matrix3d _K;
};

#endif
